#include "motor_drive.h"

Car::Car(/* args */) {
    // 设置电机的引脚的IO口和IO所绑定的通道号
    this->setMotorPin(this->m_motor[MOTOR_A], emIO_A1, emChannel_0, emIO_A2, emChannel_1);
    this->setMotorPin(this->m_motor[MOTOR_B], emIO_B1, emChannel_2, emIO_B2, emChannel_3);
    this->setMotorPin(this->m_motor[MOTOR_C], emIO_C1, emChannel_4, emIO_C2, emChannel_5);
    this->setMotorPin(this->m_motor[MOTOR_D], emIO_D1, emChannel_6, emIO_D2, emChannel_7);
}

Car::~Car() {
}

/**
 * @brief 电机初始化
 */
void Car::initMotor() {
    // 初始化GPIO口
    ledcSetup(emChannel_0, PWM_RESOLUTION, PWM_FREQUENCY);  // 建立ledc通道
    // ledcSetup(Channel_1, PWM_RESOLUTION, PWM_FREQUENCY);  // 建立ledc通道

    ledcAttachPin(A1, emChannel_0);    // 绑定通道和GOPIO口
    // ledcAttachPin(A2, Channel_1);
    ledcAttachPin(ESP32_LED, emChannel_0);    // 绑定通道和GOPIO口
}

/**
 * @brief  通道号是否合法
 * @param  channel     通道号
 * @return true        是
 * @return false       否
 */
bool Car::isChannel(const emChannelNum channel) {
    if (channel >= emChannel_0 && channel < emChannel_NULL) {
        return true;
    }
    else {
        return false;
    }
}

/**
 * @brief  控制引脚输出
 * @param  channel     ledc通道号
 * @param  output      输出模式（LOW：输出0，HIGH：输出1）
 * @param  speed       输出的速率（默认为0）
 */
void Car::pinOutput(const emChannelNum channel, const uint8_t output, const uint8_t speed) {
    uint32_t duty = this->getDutyBySpeed(speed);

    if (false == isChannel(channel)) {
        return;
    }

    if (output == LOW) {
        ledcWrite(channel, 0);
    }
    else {
        ledcWrite(channel, duty);
    }
}

/**
 * @brief  通过速率获取电机所需要的PWM信号占空比
 * @param  speed       速率
 * @return uint32_t    占空比
 */
uint32_t Car::getDutyBySpeed(uint8_t speed) {
    /*============================================================
     *
     *  以13位分辨率为例：
     *      最大duty值:（100%占空比）是8192，即 2 ^ 13
     *                  （50%占空比）是4096，即 2 ^ 13 / 2
     *      最小duty值:  （0%占空比）是0
     *
     *  m_speed传入的速度修改为0~100（滑条控件所传输的值）
     *  使用map函数将0~100映射为0~2^13的值
     *
     *============================================================*/

    return map(speed, 0, 100, 0, (1 << PWM_FREQUENCY));
}

/**
 * @brief  控制电机运动
 * @param  motorID         电机ID
 * @param  motorSpeed      电机速度
 * @param  motorDirection  电机转动的方向（0：停止  1：正转  2：反转）
 */
void Car::motorRun(const uint8_t motorID, const float motorSpeed, const uint8_t motorDirection) {
    if (motorID < MOTOR_A || motorID > MOTOR_D) {
        Serial.printf("ERR: 控制电机运动失败, motor: [%d]\n", motorID);
        return;
    }

    motor& Motor = this->m_motor[motorID];
    auto C1 = Motor.pin_1.channel;
    auto C2 = Motor.pin_2.channel;

    switch (motorDirection)
    {
        case MOTOR_MOVE_FORWARD: {
            this->pinOutput(C1, LOW);
            this->pinOutput(C2, HIGH, (uint8_t)motorSpeed);
        } break;

        case MOTOR_MOVE_REVERSE: {
            this->pinOutput(C1, HIGH, (uint8_t)motorSpeed);
            this->pinOutput(C2, LOW);
        } break;

        case MOTOR_MOVE_STOP:
        default: {
            this->pinOutput(C1, LOW);
            this->pinOutput(C2, LOW);
        } break;
    }
}

/**
 * @brief  设置电机的引脚的IO口和IO所绑定的通道号
 * @param  motor       需要设置的电机
 * @param  pin1_IO     引脚1的IO
 * @param  pin1_C      引脚1的通道
 * @param  pin2_IO     引脚2的IO
 * @param  pin2_C      引脚2的通道
 */
void Car::setMotorPin(stMOTOR& motor, emIONum pin1_IO, emChannelNum pin1_C, emIONum pin2_IO, emChannelNum pin2_C) {
    motor.pin_1.IO      = pin1_IO;
    motor.pin_1.channel = pin1_C;
    motor.pin_2.IO      = pin2_IO;
    motor.pin_2.channel = pin2_C;
}

/**
 * @brief  移动小车
 * @param  xAxis       摇杆的X轴数值
 * @param  yAxis       摇杆的Y轴数值
 */
void Car::moveCar(uint8_t xAxis, uint8_t yAxis) {

    Serial.printf("原始摇杆值为: (%d, %d)\n", xAxis, yAxis);

    float X_speed    =   xAxis - 128;
    float Y_speed    = -(yAxis - 128);
    float turn_speed = 0;
    float R          = 0;  // 计算系数

    Serial.printf("计算后: (%f, %f)\n", X_speed, Y_speed);

    float cmdSum = (abs(Y_speed) + abs(X_speed) + abs(turn_speed));  // 取绝对值求和
    if (cmdSum > 128) {
        R = 128 / cmdSum;

        Y_speed    *= R;
        X_speed    *= R;
        turn_speed *= R;
    }

    float MotorSpeedArray[MOTOR_NUM] = {
        ((Y_speed + X_speed) + turn_speed) / 1.28f,  // MA
        ((Y_speed - X_speed) - turn_speed) / 1.28f,  // MB
        ((Y_speed + X_speed) - turn_speed) / 1.28f,  // MC
        ((Y_speed - X_speed) + turn_speed) / 1.28f   // MD
    };

    int MotorArray[MOTOR_NUM] = {
        MOTOR_A, MOTOR_B, MOTOR_C, MOTOR_D
    };

    for (int i = 0; i < MOTOR_NUM; i++)
    {
        Serial.printf("MotorSpeedArray[%d]: [%f%%]\n", i, MotorSpeedArray[i]);
        if (MotorSpeedArray[i] > 0) {
            motorRun(MotorArray[i], MotorSpeedArray[i], MOTOR_MOVE_FORWARD);
        }
        else {
            if (MotorSpeedArray[i] < 0) {
                motorRun(MotorArray[i], -MotorSpeedArray[i], MOTOR_MOVE_REVERSE);
            }
            else {
                motorRun(MotorArray[i], 0, MOTOR_MOVE_STOP);
            }
        }
    }
}